/**
  ******************************************************************************
  * File Name          : air_pump.cpp
  * Description       : air_pump program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <air_pump_driver/air_pump.h>

std::string _port;
serial::Serial ser;

unsigned char rxd[RXD_MAX]; 
unsigned char txd[TXD_MAX];                 

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  接收/Gripper话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void gripperCB(const std_msgs::UInt8::ConstPtr & msg)
{
    if(msg->data==1)
    {
        txd[0]=1;
        ser.write(txd,TXD_MAX);
    }
    else
    {
        txd[0]=3;
        ser.write(txd,TXD_MAX);
    }
}

/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "air_pump");
    ros::NodeHandle nh;
    ros::Rate loop_rate(10);

    nh.getParam("/air_pump_driver/port", _port);

    //订阅夹爪话题
    ros::Subscriber sub_gripper = nh.subscribe("/gripper", 1, gripperCB);

    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 

    while (ros::ok())
    {
        if (ser.available())
        {
            ser.read(rxd, RXD_MAX);
            printf("gripper:[%d]\r\n",rxd[0]);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }

    if(ser.isOpen())
    {
        ser.close();
    }
  
    return 0;
}
